//
// Created by ze on 2024/5/25.
//
#include "UPRE_MOTOR.h"
#include "bsp_retarget.h"
#include "cmsis_os.h"
#include "vesc.h"
#include "Task scheduling_l.h"
#include "Dji_Init.h"
#include "Dji.h"
extern uint8_t receive_mode;

float vofa_trans[32];
SteeringControl_ SteeringControl = {0};


void Control_Motor() {
    // printf("%d,%f\r\n",receive_mode,Motor_param_set.shoot_ball_behind_dji2006_vel);
    osDelay(5);
    Vesc_Set_Duty(&hfdcan3, id_vesc_shoot, -Motor_param_set.shoot_ball_front_vesc_vel);///shoot ball vesc
    osDelay(5);
    // Motor_param_set.claw_storage_right_dji2006_pos=0;
    // Motor_param_set.claw_storage_left_dji2006_pos=0;
    // Motor_param_set.claw_grain_left_dji3508_pos=0;
    // Motor_param_set.claw_grain_right_dji3508_pos=-0;
//    Motor_param_set.shoot_ball_behind_dji2006_vel=3000;
//    Motor_param_set.claw_pick_ball_grasp_dji2006_pos=0;
//    Motor_param_set.claw_pick_ball_overturn_dji2006_pos=460;

    Dji_Control_can2_0x201.Dji_Pid.Tar_Pos = Motor_param_set.claw_storage_right_dji2006_pos/360.f;
    Dji_Control_can2_0x202.Dji_Pid.Tar_Pos = Motor_param_set.claw_storage_left_dji2006_pos/360.f;
    Dji_Control_can2_0x203.Dji_Pid.Tar_Pos = Motor_param_set.claw_grain_left_dji3508_pos/360.f;
    Dji_Control_can2_0x204.Dji_Pid.Tar_Pos = Motor_param_set.claw_grain_right_dji3508_pos/360.f;

    Dji_Control_can1_0x201.Dji_Pid.Tar_Vel = Motor_param_set.shoot_ball_behind_dji2006_vel;
    Dji_Control_can1_0x202.Dji_Pid.Tar_Pos = Motor_param_set.claw_pick_ball_grasp_dji2006_pos/360.f;
    Dji_Control_can1_0x203.Dji_Pid.Tar_Pos = Motor_param_set.claw_pick_ball_overturn_dji2006_pos/360.f;  ///��ʼ0
}


